



#include "sl_filter.h"

#include "sl_gauss.h"
#include<math.h>





/// <summary>
/// 初始化卡尔曼滤波器，并分配内存。
/// </summary>
/// <param name="kalman">卡尔曼滤波器</param>
/// <param name="d">状态样本的维度</param>
/// <param name="id">控制数据的维度</param>
/// <param name="od">观测数据的维度</param>
void kalman_vec_init(kalman_vec* kalman,  int d, int id, int od)
{
    kalman->state_d = d;
    kalman->input_d = id;
    kalman->observation_d = od;

    vec_allocate(&kalman->input, d);
    vec_allocate(&kalman->observe, od);

    vec_allocate(&kalman->now_state, d);
    vec_allocate(&kalman->last_state, d);
    vec_init(kalman->input, 1);
    vec_init(kalman->observe, 1);
    vec_init(kalman->now_state, 1);
    vec_init(kalman->last_state, 1);


    mat_allocate(&kalman->last_p, kalman->state_d, kalman->state_d);
    mat_allocate(&kalman->now_p, kalman->state_d, kalman->state_d);

    mat_allocate(&kalman->K, kalman->state_d, kalman->observation_d);
    mat_allocate(&kalman->Q, kalman->state_d, kalman->state_d);
    mat_allocate(&kalman->R, kalman->observation_d, kalman->observation_d);

    mat_allocate(&kalman->B, kalman->state_d, kalman->input_d);
    mat_allocate(&kalman->H, kalman->observation_d, kalman->state_d);
    mat_allocate(&kalman->F, kalman->state_d, kalman->state_d);

/*
    mat K;        //卡尔曼增益，n*m
    mat Q;        //过程噪声协方差，n*n，这个是状态转移过程的噪声。
    mat R;        //观测噪声协方差，m*m，这个是观测的噪声。

    mat B;        //控制矩阵，n*c
    mat H;        //观测矩阵，m*n
    mat F;        //状态转移矩阵，n*n
*/
  //  mat_init(kalman->K, 0); // 卡尔曼增益初始化为0.


    mat_eye(kalman->K);
    mat_eye(kalman->now_p);
    mat_eye(kalman->last_p);

    mat_eye(kalman->Q);  //
    mat_eye(kalman->R);  // 噪声初始化为单位矩阵
    mat_eye(kalman->B);
    mat_eye(kalman->H);
    mat_eye(kalman->F);  //过程，观测，控制矩阵都设为单位矩阵。如果没有特殊情况。


    // 滤波计算的中间变量
    vec_allocate(&kalman->estimate_s, kalman->state_d);
    mat_allocate(&kalman->estimate_p, kalman->state_d, kalman->state_d);
    mat_allocate(&kalman->nn, kalman->observation_d, kalman->observation_d);
    mat_allocate(&kalman->nnt, kalman->observation_d, kalman->observation_d);

    mat_allocate(&kalman->pht, kalman->state_d, kalman->observation_d);
    mat_allocate(&kalman->ht, kalman->state_d, kalman->observation_d);
    mat_allocate(&kalman->hti, kalman->state_d, kalman->observation_d);
    mat_allocate(&kalman->hpht, kalman->observation_d, kalman->observation_d);
    mat_allocate(&kalman->mm, kalman->observation_d, kalman->observation_d);
    mat_allocate(&kalman->imm, kalman->observation_d, kalman->observation_d);
    mat_allocate(&kalman->kh, kalman->state_d, kalman->state_d);
    mat_allocate(&kalman->I, kalman->state_d, kalman->state_d);

    vec_allocate(&kalman->estimate_z, kalman->observation_d);
    vec_allocate(&kalman->z_estimate_z, kalman->observation_d);

}

/// <summary>
/// 释放卡尔曼滤波器的内存。
/// </summary>
/// <param name="kalman">卡尔曼滤波器</param>
void kalman_vec_release(kalman_vec* kalman)
{
    vec_deallocate(&kalman->input);
    vec_deallocate(&kalman->observe);
    vec_deallocate(&kalman->now_state);
    vec_deallocate(&kalman->last_state);

    mat_deallocate(&kalman->last_p);
    mat_deallocate(&kalman->now_p);

    mat_deallocate(&kalman->K);
    mat_deallocate(&kalman->Q);
    mat_deallocate(&kalman->R);

    mat_deallocate(&kalman->B);
    mat_deallocate(&kalman->H);
    mat_deallocate(&kalman->F);


    // 滤波计算的中间变量
    vec_deallocate(&kalman->estimate_s);
    mat_deallocate(&kalman->estimate_p);
    mat_deallocate(&kalman->nn);
    mat_deallocate(&kalman->nnt);

    mat_deallocate(&kalman->pht);
    mat_deallocate(&kalman->ht);
    mat_deallocate(&kalman->hti);
    mat_deallocate(&kalman->hpht);
    mat_deallocate(&kalman->mm);
    mat_deallocate(&kalman->imm);
    mat_deallocate(&kalman->kh);
    mat_deallocate(&kalman->I);

    vec_deallocate(&kalman->estimate_z);
    vec_deallocate(&kalman->z_estimate_z);

}

/// <summary>
/// 根据观测数据初始化方差。用obs[]的协方差矩阵当作观测噪声和过程噪声，及初始状态的噪声。
/// </summary>
/// <param name="kalman">卡尔曼结构体</param>
/// <param name="obs">观测数据序列</param>
/// <param name="obs_len">观测数据序列长度 </param>
void kalman_vec_init_para(kalman_vec* kalman, vec obs[], int obs_len) 
{
    gauss_vec gauss;
    gauss_vec_init(&gauss, obs[0].size);
 //   gauss_vec_init(&gauss, obs, obs_len);
    mat_copyfrom(kalman->Q, gauss.covariance);
    mat_copyfrom(kalman->R, gauss.covariance);
    mat_copyfrom(kalman->now_p, gauss.covariance);
    vec_copyfrom(kalman->now_state, gauss.mean_vector);
    gauss_vec_release(&gauss);

/*
    mat Q;        //过程噪声协方差，n*n，这个是状态转移过程的噪声。
    mat R;        //观测噪声协方差，m*m，这个是观测的噪声。
*/
}

/// <summary>
/// 带控制输入的卡尔曼滤波器
/// </summary>
/// <param name="kalman">滤波器</param>
/// <param name="obs">观测矢量</param>
/// <param name="inp">控制矢量</param>
void kalman_vec_filter_with_input(kalman_vec kalman, vec obs, vec inp)
{
    vec_copyfrom(kalman.observe, obs);
    vec_copyfrom(kalman.input, inp);
    mat_copyfrom(kalman.last_p, kalman.now_p);
    vec_copyfrom(kalman.last_state, kalman.now_state);

    /*
// 两个估计的高斯分布的矢量x1和x2，协方差矩阵分别是s1和s2。
//则最优估计的协方差是  s12 = {s1^(-1)+s2^(-1)}^(-1) = s1 * {s1+s2}^(-1) * s2  , 最优估计矢量是 s12*（s1^(-1)*x2 + s2^(-1)*x1）

//卡尔曼滤波： 定义增益矩阵K = s1 * {s1+s2}^(-1).  ==>>  I-K = {s1+s2}*{s1+s2}^(-1) -s1*{s1+s2}^(-1) = s2 * {s1+s2}^(-1)
// s12 = (I-K)*s1
// x12 = (I-K)*x1 + K*x2 = x1+K*(x2-x1)

// 卡尔曼滤波的模型要复杂一些：

设系统是线性的，k时刻的状态x[k] 和k-1时刻的状态x[k-1]可以用下面的公式相联系：
x[k] = F*x[k-1]+B*u[k-1]+w[k-1]  >>   x[k]是k时刻的状态，x[k-1]是k-1时刻的状态，F是状态转移矩阵，B是控制矩阵，u是控制量，w是过程噪声。
z[k] = H*x[k]+v[k]               >>   z[k]是观测，H是观测矩阵，v是观测噪声。
过程噪声 w[k] 的协方差矩阵是Q，观测噪声 v[k] 的协方差是R。

卡尔曼滤波推导：根据k时刻观测z[k],上一个最优估计x[k-1]，过程噪声 w[k]，观测噪声 v[k]，预测最新的状态 x[k]。
预测模型：
x_[k] = F*x[k-1] + B*u[k-1] + w[k]    >>  x[k-1]是[k-1]时刻的最优估计。x_[k]是k时刻的预计，要结合观测z[k]来做最优估计。w[k]是均值为0的噪声。
P_[k] = F*P[k-1]F^T + Q               >>  Q是过程噪声w协方差，P_[k]是预测的协方差，P[k-1]是[k-1]时刻的协方差。

参数更新：按照K，x，P的顺序更新。
k时刻卡尔曼增益：      K[k] = P_[k] * H^T * ( H*P_[k]*H^T + R )^(-1)  >> 这里R是观测噪声的协方差。
k时刻预测估计：        x[k] = x_[k] + K[k]* (z[k]-H*x_[k])             >> x_[k] 是预计，x[k]是最优估计。
k时刻最优估计的协方差：P[k] = (I - K[k]*H)* P_[k]                      >> P_[k] 预计的协方差。P[k]最优估计的协方差。
*/

    mat_mul_vec(kalman.F, kalman.last_state, kalman.estimate_s); //
    mat_mul_vec(kalman.B, kalman.input, kalman.now_state);
    vec_add(kalman.estimate_s, kalman.now_state, kalman.estimate_s);   //更新预测状态  f*last_state + b*input = estimate_s 
    //  vec_print(kalman.estimate_s);
    mat_transpose_to(kalman.F, kalman.nnt);
    mat_mul(kalman.last_p, kalman.nnt, kalman.nn);
    mat_mul(kalman.F, kalman.nn, kalman.nnt);
    mat_add(kalman.nnt, kalman.Q, kalman.estimate_p);       //更新预测状态矩阵 F * (last_p * F^T) + Q = estimate_p


    mat_transpose_to(kalman.H, kalman.ht);
    mat_mul(kalman.estimate_p, kalman.ht, kalman.pht);
    mat_mul(kalman.H, kalman.pht, kalman.mm);
    mat_add(kalman.mm, kalman.R, kalman.mm);
    mat_inversion(kalman.mm, kalman.imm, Epsilon);

    mat_mul(kalman.pht, kalman.imm, kalman.K);  //更新卡尔曼增益  K[k] = P_[k] * H^T * ( H*P_[k]*H^T + R )^(-1) 

    //  mat_mul(kalman.ht, kalman.imm, kalman.hti);   这里两条语句用上面一条就行了。
    //  mat_mul(kalman.estimate_p, kalman.hti, kalman.K);   //更新卡尔曼增益  K[k] = P_[k] * H^T * ( H*P_[k]*H^T + R )^(-1) 

    mat_mul(kalman.K, kalman.H, kalman.kh);
    mat_eye(kalman.I);
    mat_sub(kalman.I, kalman.kh, kalman.I);
    mat_mul(kalman.I, kalman.estimate_p, kalman.now_p); //更新最优估计的协方差  P[k] = (I - K[k]*H)* P_[k]   

    mat_mul_vec(kalman.H, kalman.estimate_s, kalman.estimate_z);
    vec_sub(kalman.observe, kalman.estimate_z, kalman.z_estimate_z);
    mat_mul_vec(kalman.K, kalman.z_estimate_z, kalman.now_state);
    vec_add(kalman.now_state, kalman.estimate_z, kalman.now_state); //更新最优状态。    x[k] = x_[k] + K[k]* (z[k]-H*x_[k])     

    //  mat_print(kalman.K);
    //  mat_print(kalman.now_p);
}

/// <summary>
/// 无输入的卡尔曼滤波器，控制矢量初始化为0： vec_init(kalman.input, 0);
/// </summary>
/// <param name="kalman">滤波器</param>
/// <param name="obs">观测矢量</param>
void kalman_vec_filter(kalman_vec kalman, vec obs)
{
    vec_copyfrom(kalman.observe, obs);
    vec_init(kalman.input, 0);
    mat_copyfrom(kalman.last_p, kalman.now_p);
    vec_copyfrom(kalman.last_state, kalman.now_state);

    /*
// 两个估计的高斯分布的矢量x1和x2，协方差矩阵分别是s1和s2。
//则最优估计的协方差是  s12 = {s1^(-1)+s2^(-1)}^(-1) = s1 * {s1+s2}^(-1) * s2  , 最优估计矢量是 s12*（s1^(-1)*x2 + s2^(-1)*x1）

//卡尔曼滤波： 定义增益矩阵K = s1 * {s1+s2}^(-1).  ==>>  I-K = {s1+s2}*{s1+s2}^(-1) -s1*{s1+s2}^(-1) = s2 * {s1+s2}^(-1)
// s12 = (I-K)*s1
// x12 = (I-K)*x1 + K*x2 = x1+K*(x2-x1)

// 卡尔曼滤波的模型要复杂一些：

设系统是线性的，k时刻的状态x[k] 和k-1时刻的状态x[k-1]可以用下面的公式相联系：
x[k] = F*x[k-1]+B*u[k-1]+w[k-1]  >>   x[k]是k时刻的状态，x[k-1]是k-1时刻的状态，F是状态转移矩阵，B是控制矩阵，u是控制量，w是过程噪声。
z[k] = H*x[k]+v[k]               >>   z[k]是观测，H是观测矩阵，v是观测噪声。
过程噪声 w[k] 的协方差矩阵是Q，观测噪声 v[k] 的协方差是R。

卡尔曼滤波推导：根据k时刻观测z[k],上一个最优估计x[k-1]，过程噪声 w[k]，观测噪声 v[k]，预测最新的状态 x[k]。
预测模型：
x_[k] = F*x[k-1] + B*u[k-1] + w[k]    >>  x[k-1]是[k-1]时刻的最优估计。x_[k]是k时刻的预计，要结合观测z[k]来做最优估计。w[k]是均值为0的噪声。
P_[k] = F*P[k-1]F^T + Q               >>  Q是过程噪声w协方差，P_[k]是预测的协方差，P[k-1]是[k-1]时刻的协方差。

参数更新：按照K，x，P的顺序更新。
k时刻卡尔曼增益：      K[k] = P_[k] * H^T * ( H*P_[k]*H^T + R )^(-1)  >> 这里R是观测噪声的协方差。
k时刻预测估计：        x[k] = x_[k] + K[k]* (z[k]-H*x_[k])             >> x_[k] 是预计，x[k]是最优估计。
k时刻最优估计的协方差：P[k] = (I - K[k]*H)* P_[k]                      >> P_[k] 预计的协方差。P[k]最优估计的协方差。
*/

    mat_mul_vec(kalman.F, kalman.last_state, kalman.estimate_s); //
    mat_mul_vec(kalman.B, kalman.input, kalman.now_state);
    vec_add(kalman.estimate_s, kalman.now_state, kalman.estimate_s);   //更新预测状态  f*last_state + b*input = estimate_s 
  //  vec_print(kalman.estimate_s);
    mat_transpose_to(kalman.F, kalman.nnt);
    mat_mul(kalman.last_p, kalman.nnt, kalman.nn);
    mat_mul(kalman.F, kalman.nn, kalman.nnt);
    mat_add(kalman.nnt, kalman.Q, kalman.estimate_p);       //更新预测状态矩阵 F * (last_p * F^T) + Q = estimate_p


    mat_transpose_to(kalman.H, kalman.ht);
    mat_mul(kalman.estimate_p, kalman.ht, kalman.pht);
    mat_mul(kalman.H, kalman.pht, kalman.mm);
    mat_add(kalman.mm, kalman.R, kalman.mm);
    mat_inversion(kalman.mm, kalman.imm, Epsilon);

    mat_mul(kalman.pht, kalman.imm, kalman.K);  //更新卡尔曼增益  K[k] = P_[k] * H^T * ( H*P_[k]*H^T + R )^(-1) 

    //  mat_mul(kalman.ht, kalman.imm, kalman.hti);   这里两条语句用上面一条就行了。
    //  mat_mul(kalman.estimate_p, kalman.hti, kalman.K);   //更新卡尔曼增益  K[k] = P_[k] * H^T * ( H*P_[k]*H^T + R )^(-1) 

    mat_mul(kalman.K, kalman.H, kalman.kh);
    mat_eye(kalman.I);
    mat_sub(kalman.I, kalman.kh, kalman.I);
    mat_mul(kalman.I, kalman.estimate_p, kalman.now_p); //更新最优估计的协方差  P[k] = (I - K[k]*H)* P_[k]   

    mat_mul_vec(kalman.H, kalman.estimate_s, kalman.estimate_z);
    vec_sub(kalman.observe, kalman.estimate_z, kalman.z_estimate_z);
    mat_mul_vec(kalman.K, kalman.z_estimate_z, kalman.now_state);
    vec_add(kalman.now_state, kalman.estimate_z, kalman.now_state); //更新最优状态。    x[k] = x_[k] + K[k]* (z[k]-H*x_[k])     

  //  mat_print(kalman.K);
  //  mat_print(kalman.now_p);

}


/// <summary>
/// 初始化卡尔曼滤波
/// </summary>
/// <param name="q">过程噪声方差</param>
/// <param name="r">观测噪声方差</param>
/// <param name="nowp">初始状态方差，可以设大一些</param>
/// <param name="nows">当前状态</param>
void kalman_float_init(kalman_float* kalman,  float q, float r, float nowp, float nows)
{
    kalman->Q = q;
    kalman->R = r;
    kalman->now_p = nowp;
    kalman->now_state = nows;

    kalman->K = 0;

    kalman->B = 1;
    kalman->H = 1;
    kalman->F = 1;
}

/// <summary>
/// 根据观测数据初始化方差。用obs[]的协方差矩阵当作观测噪声和过程噪声，及初始状态的噪声。
/// </summary>
/// <param name="kalman">卡尔曼结构体</param>
/// <param name="obs">观测数据序列</param>
/// <param name="obs_len">观测数据序列长度 </param>
void kalman_float_init_para(kalman_float* kalman, float obs[], int obs_len)
{
    float ave = 0;
    float sum_err2 = 0;
    int i;
    for (i = 0; i < obs_len; i++) {
        ave += obs[i];
    }
    ave = ave / obs_len;

    for (i = 0; i < obs_len; i++) {
        sum_err2 += ((obs[i] - ave) * (obs[i] - ave));
    }
    sum_err2 = sum_err2 / obs_len;

    kalman->Q = sum_err2;
    kalman->R = sum_err2;
    kalman->now_p = sum_err2;
    kalman->now_state = ave;

}

/// <summary>
/// 卡尔曼滤波
/// </summary>
/// <param name="kalman">滤波器</param>
/// <param name="obs">观测量</param>
/// <param name="inp">控制量</param>
void kalman_float_filter_with_input(kalman_float* kalman, float obs, float inp)
{
    //预测：
    float estimate_s;  //预测状态
    float estimate_p;  //预测协方差     
    kalman->observe = (obs);
    kalman->input = inp;
    kalman->last_p = (kalman->now_p);
    kalman->last_state = (kalman->now_state);
    estimate_s = kalman->F * kalman->last_state + kalman->B * kalman->input;
    estimate_p = kalman->F * kalman->last_p * kalman->F + kalman->Q;
    //更新参数
    kalman->K = estimate_p * kalman->H / (kalman->H * estimate_p * kalman->H + kalman->R);
    kalman->now_state = estimate_s + kalman->K * (obs - kalman->H * estimate_s);
    kalman->now_p = (1 - kalman->K * kalman->H) * estimate_p;
}

/// <summary>
/// 卡尔曼滤波
/// </summary>
/// <param name="kalman">滤波器</param>
/// <param name="obs">观测量</param>
void kalman_float_filter(kalman_float* kalman, float obs)
{
    //预测：
    float estimate_s;  //预测状态
    float estimate_p;  //预测协方差     
    kalman->observe = (obs);
    kalman->input = 0;
    kalman->last_p = (kalman->now_p);
    kalman->last_state = (kalman->now_state);
    estimate_s = kalman->F * kalman->last_state + kalman->B * kalman->input;
    estimate_p = kalman->F * kalman->last_p * kalman->F + kalman->Q;
    //更新参数
    kalman->K = estimate_p * kalman->H / (kalman->H * estimate_p * kalman->H + kalman->R);
    kalman->now_state = estimate_s + kalman->K * (obs - kalman->H * estimate_s);
    kalman->now_p = (1 - kalman->K * kalman->H) * estimate_p;
}





#include"gl_.h"



void test_kalman_vec()
{
    kalman_vec kalman_v;
    kalman_vec_init(&kalman_v, 2, 2, 2);
    mat_mul_real(kalman_v.R, 10, kalman_v.R);

    gauss_vec gauss;
    gauss_vec_init(&gauss, 2);
    gauss.covariance.data[0][0] = 1.4;
    gauss.covariance.data[0][1] = 2;
    gauss.covariance.data[1][0] = 0;
    gauss.covariance.data[1][1] = 1.8;
    gauss_vec_renew_cov(&gauss);

#define f_len 90
    vec obs[f_len];
    vec rea[f_len];
    vec fil[f_len];
    for (int i = 0; i < f_len; i++) {
        vec_allocate(&obs[i], 2);
        vec_allocate(&rea[i], 2);
        vec_allocate(&fil[i], 2);

        rea[i].data[0] = (0.01f * i) * 10;
        rea[i].data[1] = 20 * sinf(rea[i].data[0] / 2);

        gauss_vec_randomize(gauss);
        vec_add(rea[i], gauss.random, obs[i]);
        kalman_vec_filter(kalman_v, obs[i]);
        vec_copyfrom(fil[i], kalman_v.now_state);
    }


    float x[f_len * 3];
    float y[f_len * 3];
    COLOR_RGB color[f_len * 3];
    for (int i = 0; i < f_len; i++) {
        x[i * 3] = rea[i].data[0];
        x[i * 3 + 1] = obs[i].data[0];
        x[i * 3 + 2] = fil[i].data[0];

        y[i * 3 + 0] = rea[i].data[1];
        y[i * 3 + 1] = obs[i].data[1];
        y[i * 3 + 2] = fil[i].data[1];

        color[i * 3 + 0] = RGB_lightgreen;
        color[i * 3 + 1] = RGB888_BLUE;
        color[i * 3 + 2] = RGB_red;
    }

    TwoFunctionPicture tp;
    TwoFunctionPicture_init(&tp, 0, 0, 700, 600);
    //  TwoFunctionPicture_init(&tp, 320, 400);

    char title[] = "Curve Chart";          //标题
    char xtitle[] = "xtitle time";          //x轴标题
    char y1title[] = "y1title cm";          //y轴标题
    char y2title[] = "y2title km";          //y轴标题
    TwoFunctionPicture_draw_title(&tp, title, xtitle, y1title, y2title);
    TwoFunctionPicture_draw_func(&tp,
        x, y, f_len * 3,
        NULL, NULL, 0,
        0, 0, 0, color, NULL);
    	// 按任意键继续
    GD_wait_key_input();

    for (int i = 0; i < f_len; i++) {
        vec_deallocate(&obs[i]);
        vec_deallocate(&rea[i]);
        vec_deallocate(&fil[i]);
    }
    kalman_vec_release(&kalman_v);

}


